#include <stdio.h>
#include <stdlib.h>

#include "bitstrea.h"

#include <string.h>
#ifdef __UniX
 #include <unistd.h>

#else
 //#include <mem.h>
#endif


#define CF_LE_u16(v) (v)
#define CT_LE_u16(v) (v)
#define C_ST_u16(p,v) {*(((__u16*)p)++)=v;}
#define C_LD_u16(p,v) {v=*(((__u16*)p)++);}


#ifdef DEBUG
 #define	INLINE
#else
 #ifdef _MSC_VER
  #define	INLINE _inline
 #else
  #define	INLINE inline
 #endif
#endif
//#define DEBUG	0
#define dcflDebugInfo 0x8000
#define IF_DEB if((flg&dcflDebugInfo)&&DEBUG)


const uint16_t  Hu2Sizes[8]  = {8,16,32,64,128,256,512,1024};
const uint8_t  Hu2Prefix[8] = {3, 4, 5, 6,  7,  8,  9,  10};



/*===============================*/
/*	 bitstream reading	 */


const uint16_t sq_bmsk[] =
   {0x0,0x1,0x3,0x7,0xF,0x1F,0x3F,0x7F,0xFF,
    0x1FF,0x3FF,0x7FF,0xFFF,0x1FFF,0x3FFF,0x7FFF,0xFFFF};


void FillBuffer(bits_t *pbits)
{
unsigned ReadedB;

 ReadedB=pbits->BufSize;
 if(((long)ReadedB>pbits->Total)&&(pbits->Total>0))
		 ReadedB=(unsigned)pbits->Total;

 ReadedB = fread(pbits->RealBufStart,1,ReadedB,pbits->HostFile);
 pbits->Total-=ReadedB;
 if(ReadedB==0) pbits->Total=0;	//EOF
 pbits->pd=(uint16_t*)pbits->RealBufStart;
 pbits->pe=pbits->pd+((ReadedB+1)>>1);
}


/* read next 16 bits from input*/
INLINE void RDN_G16(bits_t *pbits)
   {
    pbits->buf>>=16;
    pbits->pb-=16;

    if(pbits->pd>=pbits->pe) FillBuffer(pbits);
    (pbits)->buf |= ((uint32_t)(CF_LE_u16(*((pbits)->pd++))))<<16;

   return;
   }


/* prepares at least 16 bits for reading */
#define RDN_PR(pbits,u) \
   { \
    if((pbits)->pb>=16) RDN_G16(pbits); \
    u = (unsigned)((pbits)->buf>>(pbits)->pb); \
   }


/** Initializes reading from bitstream. BufSize must be >=4! */
void sq_rdinit(bits_t *pbits,void *BufStart,unsigned BufSize,unsigned long TotalLin,FILE *f)
{
  pbits->HostFile = f;
  pbits->Total = TotalLin;
  pbits->BufSize = BufSize & ~1;
  pbits->RealBufStart = (uint8_t *)BufStart;
  pbits->pb = 32;
  pbits->pe = (uint16_t *)BufStart;           //Buffer is Empty
  pbits->pd = pbits->pe;
}

/* reads n<=16 bits from bitstream *pbits */
uint16_t sq_rdn(bits_t *pbits, int n)
{
  uint16_t u;
  RDN_PR(pbits,u);
  pbits->pb += n;
  u &= sq_bmsk[n];
  return u;
}

/* read unlimited number of bits from bitstream *pbits */
void sq_rdUnl(bits_t *pbits,uint8_t *array,int n)
{
 while(n>=8)
	{
	*array=sq_rdn(pbits,8);
	array++;
	n-=8;
	}
 if(n>0) *array=sq_rdn(pbits,n);
 return;
}

/*===============================*/
/*	 bitstream writing	 */

/* initializes writing to bitstream. BufSize must be >=4! */
void sq_wrinit(bits_t *pbits,void *BufStart,unsigned BufSize,unsigned long TotalLin,FILE *f)
{
  pbits->HostFile=f;
  pbits->Total=TotalLin;
  pbits->BufSize=BufSize & ~1;
  pbits->RealBufStart=(uint8_t *)BufStart;
  pbits->pd=(uint16_t *)pbits->RealBufStart;
  pbits->pb=0;
  pbits->buf=0;
  pbits->pe=(uint16_t *)pbits->RealBufStart+((BufSize+1)>>1); //Buffer is Empty
}


void FlushBuffer(bits_t *pbits)
{
unsigned WritedB;

 WritedB=(uint8_t *)pbits->pd - pbits->RealBufStart;
 if(WritedB>pbits->BufSize) WritedB=pbits->BufSize;
 if(WritedB>pbits->Total) WritedB=(unsigned)pbits->Total;

 pbits->Total-=WritedB;
 fwrite(pbits->RealBufStart,1,WritedB,pbits->HostFile);

 pbits->pd=(uint16_t*)pbits->RealBufStart;
 pbits->pe=pbits->pd+((pbits->BufSize+1)>>1);
}


/** This procedure flushes even incomplete bits to the edge of byte. */
void TotalFlush(bits_t *pbits)
{
 while(pbits->pb > 0)
	{
	if(pbits->pd>=pbits->pe) FlushBuffer(pbits);
	*(pbits->pd++)=(uint16_t)pbits->buf;
        if(pbits->pb < 16) pbits->pb=0;
        	      else pbits->pb-=16;
        pbits->buf>>=16;
	}
 FlushBuffer(pbits);
}


/** write n<=16 bits to bitstream *pbits */
void sq_wrn(bits_t *pbits,unsigned u,int n)
{
  if(pbits->pb>=16)
	{
	if(pbits->pd>=pbits->pe) FlushBuffer(pbits);
	*(pbits->pd++)=(uint16_t)pbits->buf;
	pbits->pb-=16;
	pbits->buf>>=16;
	}

  pbits->buf |= (uint32_t)(u & sq_bmsk[n]) << pbits->pb;
  pbits->pb+=n;
}


/** read unlimited number of bits from bitstream *pbits */
void sq_wrUnl(bits_t *pbits,uint8_t *array,int n)
{
 while(n>=8)
	{
	sq_wrn(pbits,*array,8);
	array++;
	n-=8;
	}
 if(n>0) sq_wrn(pbits,*array,n);
 return;
}


//--------------Huffman-------------------

#include "xlat_swp.h"


/** swap 16 bits order */
INLINE unsigned swap_bits_order_16(unsigned d)
{
unsigned r;
  #ifdef USE_GNU_ASM_i386
    __asm__ (
	"xlatl\n\t"
	"xchgb	%%al,%%ah\n\t"
	"xlatl\n\t"
	:"=a"(r):"0"(d),"b"(swap_bits_xlat));
  #else
    r=((uint16_t)swap_bits_xlat[(uint8_t)d])<<8;
    r|=swap_bits_xlat[(uint8_t)(d>>8)];
  #endif
  return r;
}

/* swap bit order */
INLINE unsigned swap_bits_order(unsigned d,int n)
{ unsigned r=0;
  while(n--) { r<<=1;r|=d&1;d>>=1;}
  return r;
}


/* initializes huffman conversion structure *phuf for m codes,
   *ca code and token bit lengths, ends with 0xFF,
   bn predicated maximal bit length */
int sq_rdhufi(huf_rd_t *phuf,int m,int bn,uint8_t *ca)
{
 if(bn>MAX_SPDA_BITS) bn=MAX_SPDA_BITS;
 phuf->bn=bn;
 {
  int i;
  unsigned u,us,ut;
  memset(phuf->cd_ln,0,sizeof(phuf->cd_ln));i=0;
  while((u=ca[i++])<=MAX_BITS) phuf->cd_ln[u]++;
  memset(phuf->cd_ch,0,sizeof(phuf->cd_ch));
  phuf->cd_ln[0]=0;us=0;ut=0;
  for(i=1;i<=MAX_BITS;i++)
  {
   u=phuf->cd_ln[i];phuf->cd_ln[i]=ut;
   phuf->cd_ch[i]=us;ut+=u;us+=u;us<<=1;
  }
  /* if suceed us should be 0x10000 */
  if (us&((1<<MAX_BITS)-1)) return(0);
 }
 {
  int i,ln,l,ch,sh,cod;
  for(i=0;(l=ln=ca[i])<=MAX_BITS;i++) if(ln)
  {
   sh=(bn-ln);
   cod=(phuf->cd_ch[ln])++;
   cod=swap_bits_order_16(cod)>>(16-ln);
   if(i<m) ch=i; else {ch=i-m+1;ln-=0x40;}
   if (sh>0)
   {
    sh=1<<sh;
    l=1<<l;
    while(sh--)
    {
      phuf->chln[cod].ch=ch;
      phuf->chln[cod].ln=ln;
      cod+=l;
    }
   } else if (sh==0) {
    phuf->chln[cod].ch=ch;
    phuf->chln[cod].ln=ln;
   } else {
    cod&=sq_bmsk[bn];
    phuf->chln[cod].ch=0x00;
    phuf->chln[cod].ln=-0x40;
    cod=(phuf->cd_ln[l])++;
    phuf->chln1[cod].ch=ch;
    phuf->chln1[cod].ln=ln;
   }
  }
  /* if suceed ln should be 0xFF */
 }
 return(1);
}


/* read one huffman encoded value */
unsigned sq_rdh1(bits_t *pbits,const huf_rd_t *phuf)
{unsigned ch;
 if(pbits->pb>=16) RDN_G16(pbits);
 ch=(unsigned)((pbits->buf>>pbits->pb)&sq_bmsk[phuf->bn]);
 if((pbits->pb+=phuf->chln[ch].ln)>=0) return phuf->chln[ch].ch;
 ch=phuf->chln[ch].ch;
 pbits->pb+=0x40; if(ch) return ch+0x100-1; /*F offset tokenu */
 ch=swap_bits_order_16((uint16_t)(pbits->buf>>pbits->pb));
 {int i;
  i=phuf->bn;
  do
   i++;			/* tady lze nacitat kody delsi nez 16 */
  while((phuf->cd_ch[i]<=(ch>>(16-i)))&&(i<MAX_BITS));
  ch=((ch>>(16-i)))-phuf->cd_ch[i]+phuf->cd_ln[i];
 }
 if((pbits->pb+=phuf->chln1[ch].ln)>=0) return phuf->chln1[ch].ch;
 pbits->pb+=0x40;
 return phuf->chln1[ch].ch+0x100-1; /*F offset tokenu */
}


/* ****** huffman coding *******  */


/*** Hacked generation of character codes ***/
/* Not working */

void sq_sortp(ch_tab_t* ch_tab,int ch_num)
{
 int i;
 for(i=0;i<ch_num;i++)
 {
/*  printf("%4X %4X ",(int)ch_tab[i].ch,(int)ch_tab[i].cn); */
  printf("%X:%X=%X ",((int)ch_tab[i].ch>>11)&0x1F,((int)ch_tab[i].ch)&0x3FF,(int)ch_tab[i].cn);
 }
 printf("\n");
}


void sq_hsort1(ch_tab_t* ch_tab,int ch_num,int cl, ch_tab_t a)
{
 /* a           eax */
 /* cl          di  */
 int ch;     /* bp  */
 ch_tab_t b; /* ecx */
 ch_tab_t c; /* esi */
 ch=cl*2;
 while(ch<ch_num)
 {
  b=ch_tab[ch-1];c=ch_tab[ch];
  if((c.cn<b.cn)||((c.cn==b.cn)&&(c.ch<=b.ch))) {b=c;ch++;}
  if((b.cn>a.cn)||((b.cn==a.cn)&&(b.ch>=a.ch))) {ch_tab[cl-1]=a;return;}
  ch_tab[cl-1]=b;cl=ch;ch*=2;
 }
 if(ch==ch_num)
 {
  b=ch_tab[ch-1];
  if((b.cn<a.cn)||((b.cn==a.cn)&&(b.ch<a.ch)))
   {ch_tab[cl-1]=b;cl=ch;ch*=2;}
 }
 ch_tab[cl-1]=a;
}


/** @param[in,out]	ch_cn	Historam of symbols, might be reduced on output. Size cod_num.
 *  @param[in,out]	ch_blen Symbol bit length. Should be zero initialised from caller.
 *  @param[in]		cod_num Amount of symbols. */
int sq_huffman(count_t* ch_cn, uint8_t* ch_blen, unsigned* ch_blcn, const int cod_num, ch_tab_t *ch_tab)
{
 int i,ch_num,cl;
 ch_tab_t a;
 ch_tab_t b;

redo_reduced:
 ch_num=0;
 for(i=0;i<cod_num;i++)
   if(ch_cn[i])
     {ch_tab[ch_num].cn=ch_cn[i];ch_tab[ch_num].ch=i|0x800;ch_num++;}
 ch_tab[ch_num].ch=0;
 if(ch_num==1)
 {
  ch_tab[ch_num]=ch_tab[ch_num-1];
  ch_tab[ch_num].ch&=0x801;
  ch_tab[ch_num].ch^=1;ch_num++;
 }
 cl=ch_num/2;
 while(cl>1)
 {
  sq_hsort1(ch_tab,ch_num,cl,ch_tab[cl-1]);
  cl--;
 }

 cl=ch_num; a=ch_tab[0];
 while(cl>2)
 {
  sq_hsort1(ch_tab,cl,1,a);
  b=ch_tab[0];
  a=ch_tab[--cl];
  ch_tab[cl].ch=b.ch;
  sq_hsort1(ch_tab,cl,1,a);
  a=ch_tab[0];
  ch_tab[cl].cn=a.ch;
  if(a.ch<=b.ch) {a.ch=b.ch;}
  a.ch=(a.ch&0x7800)+cl+0x800;
  if(a.ch>=0x8000u)
  {
   printf("sd4_huffman: Problems with number of bits\n");
   for(i=0;i<cod_num;i++) ch_cn[i]=(ch_cn[i]+1)>>1;
   goto redo_reduced;
  }
  a.ch+=0x8000u;
  a.cn+=b.cn;
 }
 ch_tab[1].cn=a.ch;

 {
  int st[MAX_BITS+2];
  int k=0,l=1,blen=0;

  memset(ch_blcn,0,sizeof(ch_blcn[0])*16);
  //memset(ch_blen,0,cod_num);			///< should be initialised from caller
  while(1)
  {
   do
   {
    k|=0x4000;
    do
    {
     st[blen]=k;
     blen++;
     k=l&0x7FF;
     l=ch_tab[k].ch&0x87FF;
    }while(l&0x8000);
    ch_blen[l]=blen;
    ch_blcn[blen]++;
    l=ch_tab[k].cn&0x87FF;
   }while(l&0x8000);
   do
   {
    ch_blen[l]=blen;
    ch_blcn[blen]++;
    do
    {
     if(!--blen) goto code_done;
     k=st[blen];
    }while(k&0x4000);
    l=ch_tab[k].cn&0x87FF;
   }while(!(l&0x8000));
  }
code_done:;
 }
 return(0);
}


int sq_wrhufi(huf_wr_t *phuf, unsigned* ch_blencn,int cod_num)
{
 unsigned i,u,t,blen;
 u=0;
 for(i=0;i<16;i++)
   {u<<=1;t=u;u+=ch_blencn[i];ch_blencn[i]=t;}
 if(u!=0x8000u) return(1);
 for(i=0;i<cod_num;i++)
 {
  if((blen=phuf->ln[i])!=0)
  {
   phuf->cod[i]=swap_bits_order_16(ch_blencn[blen]++)>>(16-blen);
  }
 }
 return(0);
}


//----------------------------------------




//-----------------------------------------





/*
void main(void)
{
FILE *pokus;
bits_t bity;
int pocet;

char buffer[50]={1,2,3,4,5,6,7,8,9,11,12,13,14,15,16,17,18,19,
		 21,22,23,24,25,26,27,28,29}


 sq_rdi(&bity,buffer,20);

 do {
    printf("pocet bitu:");
    scanf("%d",&pocet);

    printf(" prectene cislo:%x \n\r",sq_rdn(&bity,pocet));

    }while(1);




 pokus=fopen("leaves.ftg","r");
 if (pokus==NULL) return;         //File cannot be opened

 fclose(pokus);
 return;
}
*/

